#include "serial_driver/JLinkInterface.hpp"
#include <cstdio>
#include <ros/ros.h>

namespace rm_serial_driver {

// 自定义的回调函数，阻止显示不安全对话框
int MyUnsecureDialog(const char* sTitle, const char* sMsg, uint32_t Flags) {
    // 我们不做任何事，这样就不会显示对话框
    printf("Unsecure Dialog blocked");
    // 返回值为 0 或其他值，表示没有选择任何按钮，阻止对话框
    return 0;  // 阻止弹窗，返回 0
}

// 自定义错误输出处理函数
void MyErrorOutHandler(const char* sError) {
    printf("Custom error handler: %s", sError);
}

void MyWarnOutHandler(const char* sError) {
    printf("Custom warn handler: %s", sError);
}

JLinkOperations::JLinkOperations(const char* lib_path, const char *deviceName):deviceName(deviceName) {
    // 加载动态库
    hJLinkDLL = dlopen(lib_path, RTLD_NOW);
    if (!hJLinkDLL) {
        printf("Failed to load J-Link library: %s\n", dlerror());
        exit(1);
    }

    // 获取 J-Link 函数指针
    JLINKARM_Open = (JLINKARM_OPEN_FUNC) dlsym(hJLinkDLL, "JLINKARM_Open");
    JLINKARM_Close = (JLINKARM_CLOSE_FUNC) dlsym(hJLinkDLL, "JLINKARM_Close");
    JLINKARM_TIF_Select = (JLINKARM_TIF_SELECT_FUNC) dlsym(hJLinkDLL, "JLINKARM_TIF_Select");
    JLINKARM_Connect = (JLINKARM_CONNECT_FUNC) dlsym(hJLinkDLL, "JLINKARM_Connect");
    JLINKARM_ReadMem = (JLINKARM_READMEM_FUNC) dlsym(hJLinkDLL, "JLINKARM_ReadMem");
    JLINKARM_SetSpeed = (JLINKARM_SetSpeed_FUNC) dlsym(hJLinkDLL, "JLINKARM_SetSpeed");
    JLINKARM_ExecCommand = (JLINKARM_EXEC_COMMAND_FUNC) dlsym(hJLinkDLL, "JLINKARM_ExecCommand");
    JLINK_RTTERMINAL_Control = (JLINK_RTTERMINAL_CONTROL_FUNC) dlsym(hJLinkDLL, "JLINK_RTTERMINAL_Control");
    JLINK_RTTERMINAL_Read = (JLINK_RTTERMINAL_READ_FUNC) dlsym(hJLinkDLL, "JLINK_RTTERMINAL_Read");
    JLINK_RTTERMINAL_Write = (JLINK_RTTERMINAL_WRITE_FUNC) dlsym(hJLinkDLL, "JLINK_RTTERMINAL_Write");
    JLINKARM_IsOpen = (JLINKARM_ISOPEN_FUNC) dlsym(hJLinkDLL, "JLINKARM_IsOpen");
    JLINKARM_EmuIsConnected = (JLINKARM_EMU_ISCONNECTED_FUNC) dlsym(hJLinkDLL, "JLINKARM_EmuIsConnected");
    JLINKARM_IsConnected = (JLINKARM_ISCONNECTED_FUNC) dlsym(hJLinkDLL, "JLINKARM_IsConnected");
    JLINKARM_EmuGetList = (JLINKARM_EMU_GETLIST_FUNC) dlsym(hJLinkDLL, "JLINKARM_EMU_GetList");
    JLINK_SetHOOK_Dialog_UNSECURE = (JLINK_SetHOOK_DIALOG_UNSECURE_FUNC) dlsym(hJLinkDLL, "JLINK_SetHookUnsecureDialog");
    JLINKARM_SetErrorOutHandle = (JLINKARM_SET_ERROR_OUT_HANDLE_FUNC) dlsym(hJLinkDLL, "JLINKARM_SetErrorOutHandler");
    JLINKARM_SetWarningOutHandle = (JLINKARM_SET_WARNING_OUT_HANDLE_FUNC) dlsym(hJLinkDLL, "JLINKARM_SetWarnOutHandler");
}

JLinkOperations::~JLinkOperations() {
    // 关闭动态库
    dlclose(hJLinkDLL);
}

bool JLinkOperations::initializeJLink() {
    if (!JLINKARM_Open || !JLINKARM_Close || !JLINKARM_TIF_Select || !JLINKARM_Connect ||
        !JLINKARM_ReadMem || !JLINKARM_SetSpeed || !JLINK_RTTERMINAL_Control ||
        !JLINK_RTTERMINAL_Read || !JLINK_RTTERMINAL_Write) {
        printf("Failed to find required functions: %s\n", dlerror());
        dlclose(hJLinkDLL);
        exit(1);
    }
    JLINKARM_SetErrorOutHandle(MyErrorOutHandler); // 设置自定义错误输出处理函数
    JLINKARM_SetWarningOutHandle(MyWarnOutHandler);

    if (JLINKARM_Open() != 0) {
        printf("JLINKARM_Open failed\n");
        exit(1);
    }

    JLINKARM_TIF_Select(1);
    JLINKARM_SetSpeed(20000);

    // char acIn[0x400];
    // char acOut[0x400];
    snprintf(acIn, sizeof(acIn), "device = %s", deviceName);
    JLINKARM_ExecCommand((unsigned char *)acIn, (unsigned char *)acOut, sizeof(acOut));
    
    if (JLINKARM_Connect() != 0) {
        printf("JLINKARM_Connect failed\n");
        exit(1);
    }

    JLINK_SetHOOK_Dialog_UNSECURE(MyUnsecureDialog); // 设置自定义回调函数，阻止不安全对话框
    
    return true;
}

void JLinkOperations::close() {
    JLINKARM_Close();
}

bool JLinkOperations::readMemory(unsigned int address, void *buffer, unsigned int numBytes) {
    if (JLINKARM_ReadMem) {
        return JLINKARM_ReadMem(address, numBytes, buffer) == 0;
    }
    return false;
}

bool JLinkOperations::writeMemory(unsigned int address, const void *buffer, unsigned int numBytes) {
    // 写内存可以扩展，需要时添加相应函数支持
    return false;
}

bool JLinkOperations::startRTT() {
    int command = RTT_COMMAND_START;
    void* config = nullptr;
    int result = JLINK_RTTERMINAL_Control(command, config);
    if (result < 0) {
        printf("RTT start failed: %d\n", result);
        return false;
    }
    return true;
}

void JLinkOperations::stopRTT() {
    int command = RTT_COMMAND_STOP;
    void* config = nullptr;
    int result = JLINK_RTTERMINAL_Control(command, config);
    if (result < 0) {
        printf("RTT stop failed: %d\n", result);
        exit(1);
    }
}

int JLinkOperations::ifConnected() {
    if (JLINKARM_IsOpen() == 0) {
        return 0;
    }
    else {
        return 1;
    }
}

int JLinkOperations::ifEmuConnected() {
    if (JLINKARM_EmuIsConnected() == 0) {
        return 0;
    }
    else {
        return 1;
    }
}

bool JLinkOperations::isConnected() {
    return JLINKARM_IsConnected ? JLINKARM_IsConnected() == 1 : false;
}

bool JLinkOperations::reInit()
{
    if (JLINKARM_Open() != 0) {
        ROS_INFO(  "Failed to open J-Link connection.\n");
        return false;
    }
    ROS_INFO(  " success to open J-Link connection.\n");

    JLINKARM_TIF_Select(1); // 选择 SWD 接口
    ROS_INFO(  " sucstdhbsdgbsdthnsdtn\n");

    JLINKARM_SetSpeed(20000);
    ROS_INFO(  " success to set speed.\n");

    snprintf(acIn, sizeof(acIn), "device = %s", deviceName);
    JLINKARM_ExecCommand((unsigned char *)acIn, (unsigned char *)acOut, sizeof(acOut));
    ROS_INFO(  " success to exec command.\n");

    if (JLINKARM_Connect() != 0) {
        ROS_INFO(  "Failed to connect to target device.\n");
        return false;
    }
    ROS_INFO(  "success to connect to target device.\n");

    return true;
}

bool JLinkOperations::ifHaveUSB()
{
    JLINKARM_EMU_CONNECT_INFO connectInfo[2];
    int numDevices = JLINKARM_EmuGetList(JLINKARM_HOSTIF_USB, connectInfo, 2);
    if (numDevices > 0) {
        // printf("found %d USB devices\n", numDevices);
        return true;
    }
    else {
        return false;
    }
}

int JLinkOperations::readRTT(int bufferIndex, void* buf, int numBytes) {
    return JLINK_RTTERMINAL_Read(bufferIndex, buf, numBytes);
}

int JLinkOperations::writeRTT(int bufferIndex, const void* buf, int numBytes) {
    return JLINK_RTTERMINAL_Write(bufferIndex, buf, numBytes);
}

}